#pragma once

#include "pipea.hpp"

#include "fiocpp.hpp"

class FinderBase
{
public:
	std::string winname;
	cv::Mat frame;	
	cv::Point pt;

	cv::Vec6f idlepos;

public:
	FinderBase()
		: idlepos(0.090f, -0.417f, 0.339f, 1.24f, 2.83f, -0.07f)
	{
		winname = "frame";
	}

	static bool SavePos(const float p[], size_t n, std::string fp)
	{
		std::ofstream ofs(fp);
		if (ofs)
		{
			for (size_t i = 0; i < n; i++)
				ofs << p[i] << '\n';
			ofs.close();
			return true;
		}
		return false;
	};


	static bool LoadPos(float p[], size_t n, std::string fp)
	{
		std::ifstream ifs(fp);
		if (ifs)
		{
			for (size_t i = 0; i < n; i++)
				ifs >> p[i];
			ifs.close();
			return true;
		}
		return false;
	};



	static bool FindCorners(std::vector<cv::Point2f> &corners, const cv::Mat &frame, cv::Size patternsize = cv::Size(6, 9))
	{
		cv::Mat gray;
		cv::cvtColor(frame, gray, CV_BGR2GRAY);

			//this will be filled by the detected corners
										  //CALIB_CB_FAST_CHECK saves a lot of time on images
										  //that do not contain any chessboard corners
		bool patternfound = cv::findChessboardCorners(gray, patternsize, corners,
			cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE
			//    + cv::CALIB_CB_FAST_CHECK
		);

		if (!patternfound)
			return false;

		cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
			cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

		cv::Mat canvas;
		frame.copyTo(canvas);
		cv::drawChessboardCorners(canvas, patternsize, cv::Mat(corners), patternfound);
		cv::imshow("FF", canvas);

		return true;
	}


	virtual bool ComputePath(std::vector<cv::Vec6f> &path)
	{
		return true;
	}

	static void onMouse(int event, int x, int y, int flags, void* userdata)
	{
		if (event != cv::EVENT_LBUTTONDBLCLK)
			return;

		FinderBase *pf = (FinderBase*)userdata;

		pf->pt = cv::Point(x, y);

		std::vector<cv::Vec6f> p;

		if (pf->ComputePath(p) && !p.empty())
		{
			PipeA pa;
			pa.SetPath(p);
		}
	}

	bool ManualPath(int index = 0)
	{
		cv::VideoCapture c(index);
		if (c.isOpened())
		{
			c.set(CV_CAP_PROP_FRAME_WIDTH, 640);
			c.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
			c.set(CV_CAP_PROP_SETTINGS, 1);
			cv::namedWindow(winname);
			cv::setMouseCallback(winname, onMouse, this);

			pt = cv::Point(-1, -1);

			PipeA pa;

			while (c.read(frame))
			{		
				cv::Mat canvas;
				frame.copyTo(canvas);
				cv::circle(canvas, pt, 4, cv::Scalar(0, 0, 255));
				cv::imshow(winname, canvas);

				int k=cv::waitKey(1);

				switch (k)
				{
				case 27:
					return true;
				case 'i':
					pa.SetPos(idlepos);				
					break;
				case 'c':
				{
					cv::Vec6f pos;
					pa.GetPos(pos);
					std::string tname = TimeString();
					cv::imwrite(tname + ".png", frame);
					SavePos(pos.val, 6, tname + ".txt");					
				}
				break;
				default:
					break;
				}
			}
		}
		return false;
	}

	bool AutoPath(int index = 0)
	{
		cv::VideoCapture c(index);
		if (c.isOpened())
		{
			c.set(CV_CAP_PROP_FRAME_WIDTH, 640);
			c.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
			//c.set(CV_CAP_PROP_SETTINGS, 1);
			while (c.read(frame))
			{
				std::vector<cv::Vec6f> p;
				if (ComputePath(p) && !p.empty())
				{
					PipeA pa;
					pa.SetPath(p);
					return true;
				}
			}
		}
		return false;
	}


};